#include "can_common.h"
#include "dji.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robot_cmd.h"
#include "rtdef.h"
#include "rtthread.h"
#include "rttypes.h"
#include "robotdef.h"

DJIMotor_t             *friction_l, *friction_r, *loader; // 拨盘电机
static Publisher_t     *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv;                   // 来自cmd的发射控制信息
static Subscriber_t    *shoot_sub;

#define LOG_TAG "shoot"
#define LOG_LVL LOG_LVL_DBG
#include <ulog.h>
void ShootInit()
{
    // 左摩擦轮
    Motor_Init_Config_s friction_config = {
        .offline_manage_motor ={
            .event = OFFLINE_SHOOT_MOTOR_LEFT,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 3,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
                            .can_handle = &can2_bus,
                            },
        .controller_param_init_config = {
                            .speed_PID = {
                .Kp            = 20, // 20
                .Ki            = 200, // 1
                .Kd            = 0,
                .Improve       = PID_Integral_Limit,
                .IntegralLimit = 10000,
                .MaxOut        = 15000,
            },
                            .current_PID = {
                .Kp            = 0.1, // 0.7
                .Ki            = 0.1, // 0.1
                .Kd            = 0,
                .Improve       = PID_Integral_Limit,
                .IntegralLimit = 10000,
                .MaxOut        = 15000,
            },
                            },
        .controller_setting_init_config = {
                            .angle_feedback_source = MOTOR_FEED,
                            .speed_feedback_source = MOTOR_FEED,

                            .outer_loop_type    = SPEED_LOOP,
                            .close_loop_type    = SPEED_LOOP | CURRENT_LOOP,
                            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
                            },
        .motor_type = M3508
    };
    friction_config.can_init_config.tx_id = 1,
    //friction_l                            = DJIMotorInit(&friction_config);

    friction_config.can_init_config.tx_id                             = 2; // 右摩擦轮,改txid和方向就行
    friction_config.offline_manage_motor.event                        = OFFLINE_SHOOT_MOTOR_RIGHT;
    friction_config.offline_manage_motor.beep_times                   = 4;
    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    //friction_r                                                        = DJIMotorInit(&friction_config);

    // 拨盘电机
    Motor_Init_Config_s loader_config = {
        .offline_manage_motor ={
            .event = OFFLINE_SHOOT_MOTOR_RAMMER,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 5,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
                            .can_handle = &can1_bus,
                            .tx_id      = 7,
                            },
        .controller_param_init_config = {
                            .angle_PID = {
                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
                .Kp     = 10, // 10
                .Ki     = 0,
                .Kd     = 0,
                .MaxOut = 200,
            },
                            .speed_PID = {
                .Kp            = 10, // 10
                .Ki            = 100, // 1
                .Kd            = 0,
                .Improve       = PID_Integral_Limit,
                .IntegralLimit = 7000,
                .MaxOut        = 10000,
            },
                            .current_PID = {
                .Kp            = 0.1, // 0.7
                .Ki            = 0, // 0.1
                .Kd            = 0,
                .Improve       = PID_Integral_Limit,
                .IntegralLimit = 7000,
                .MaxOut        = 10000,
            },
                            },
        .controller_setting_init_config = {
                            .angle_feedback_source = MOTOR_FEED,
                            .speed_feedback_source = MOTOR_FEED,
                            .outer_loop_type       = SPEED_LOOP,             // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
            .close_loop_type       = CURRENT_LOOP | SPEED_LOOP,
                            .motor_reverse_flag    = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
        },
        .motor_type = M2006  // 英雄使用m3508
    };
    //loader = DJIMotorInit(&loader_config);

    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
}

/* 机器人发射机构控制核心任务 */
static void shootask(void *parameter)
{
    ShootInit();
    //DJIMotorEnable(friction_l);
    DJIMotorEnable(friction_r);
    DJIMotorEnable(loader);
    while (1)
    {
        // 从cmd获取控制数据
        SubGetMessage(shoot_sub, &shoot_cmd_recv);

        //确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
        if (shoot_cmd_recv.friction_mode == FRICTION_ON)
        {
            // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
            //DJIMotorSetRef(friction_l, 4000);
            DJIMotorSetRef(friction_r, 20000);
        }
        else // 关闭摩擦轮
        {
            //DJIMotorSetRef(friction_l, 0);
            DJIMotorSetRef(friction_r, 0);
            //DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
            //DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
        }

        switch (shoot_cmd_recv.load_mode)
        {
        // 停止拨盘
        case LOAD_STOP:
            DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
            DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
            break;
        // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
        case LOAD_1_BULLET:                                                               // 激活能量机关/干扰对方用,英雄用.
            DJIMotorOuterLoop(loader, ANGLE_LOOP);                                        // 切换到角度环
            DJIMotorSetRef(loader, loader->measure.angle_single_round + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
            break;
        // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
        case LOAD_BURSTFIRE:
            DJIMotorOuterLoop(loader, SPEED_LOOP);
            DJIMotorSetRef(loader, 25000);
            // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
            break;
        // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
        // 也有可能需要从switch-case中独立出来
        case LOAD_REVERSE:
            DJIMotorOuterLoop(loader, SPEED_LOOP);
            // ...
            break;
        }



        // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
        rt_thread_mdelay(10);
    }
}

int shoot_thread_entry_init(void)
{
    rt_err_t    ret    = RT_EOK;
    rt_thread_t thread = rt_thread_create("shoot", shootask, RT_NULL, 4096, 31, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("shoot thread init success!\n");

    return ret;
}
#ifdef BOARD_SMALL_YAW_1 
INIT_APP_EXPORT(shoot_thread_entry_init);
#endif 

#ifdef BOARD_SMALL_YAW_2
INIT_APP_EXPORT(shoot_thread_entry_init);
#endif 

